#include <ESP32Servo.h>

Servo myservo;  // create servo object to control a servo
// 16 servo objects can be created on the ESP32

int pos = 0;    // variable to store the servo position


int servoPin = 5;

int pingPin = 39; // Trigger Pin of Ultrasonic Sensor超声波
int echoPin = 37; // Echo Pin of Ultrasonic Sensor超声波

int get_distance(int pingPin,int echoPin)
{
   long duration, inches, cm;
   pinMode(pingPin, OUTPUT);
   digitalWrite(pingPin, LOW);
   delayMicroseconds(2);
   digitalWrite(pingPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(pingPin, LOW);
   pinMode(echoPin, INPUT);
   duration = pulseIn(echoPin, HIGH);
   cm = microsecondsToCentimeters(duration);
   cm = constrain(cm, 0, 255);
   
   return cm;  
}

long microsecondsToCentimeters(long microseconds) 
{
   return microseconds / 29 / 2;
}

void setup() 
{
  Serial.begin(9600);
  // Allow allocation of all timers
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  myservo.setPeriodHertz(50);    // standard 50 hz servo
  myservo.attach(servoPin, 500, 2500); // attaches the servo on pin 18 to the servo object
  // using default min/max of 1000us and 2000us
  // different servos may require different min/max settings
  // for an accurate 0 to 180 sweep
  myservo.write(0);    // tell servo to go to position in variable 'pos'
  delay(500);             // waits 15ms for the servo to reach the position
}

int d1,d2;

int p = 0;
int i;


void loop() 
{
  d1 = get_distance(pingPin,echoPin);
  delay(50);
  d2 = get_distance(pingPin,echoPin);
  delay(50);
  //Serial.println(d1);
  if(d1 <= 15 && d2 <=15 && p == 0)
  {
    p = 90;
    myservo.write(p);    // tell servo to go to position in variable 'pos'
    delay(1000);             // waits 15ms for the servo to reach the position    
  }
  if(d1 > 15 && d2 > 15 && p == 90)
  {
    p = 0;
    myservo.write(p);    // tell servo to go to position in variable 'pos'
    delay(500);             // waits 15ms for the servo to reach the position
  }
   
  Serial.println(get_distance(pingPin,echoPin));
  
  
}
